#include "task_main.h"

int main(int argc, char** argv)
{
    ros::init(argc, argv, "TaskMain");
    TaskMain node;
    node.init();
    node.run();
    ros::spin();
    return 0;
}

TaskMain::TaskMain()
{
    // Constructor implementation
}

TaskMain::~TaskMain()
{
    // Destructor implementation
}

void TaskMain::init()
{
    // Initialization code
    carto_sub = nh_.subscribe<geometry_msgs::PoseStamped>("/tracked_pose", 1, &TaskMain::GetCartographPoseCmdCallback, this);
    test_sub = nh_.subscribe("/dm_test", 1, &TaskMain::GetTestCmdCallback, this);
    params = std::make_shared<Params>();
    tools = std::make_shared<Tools>();
    nav_controller = std::make_shared<Navigation>(params);
    pcl_match = std::make_shared<PclMatch>(params);

    float min_angle, min_distance, max_distance;
    int step,size_threshold;
    // nh_.param<float>("min_distance", min_distance, 0.01); 
    // nh_.param<float>("max_distance", max_distance, 0.1); 
    // nh_.param<float>("min_angle", min_angle, 0.2); 
    // nh_.param<int>("size_threshold", size_threshold, 100); 
    // nh_.param<int>("step", step, 10); 
    if (!nh_.getParam("task_main/min_distance", min_distance)) {
        ROS_ERROR("Failed to get param 'min_distance'");
        min_distance = 0.01; // 默认值
    }
    if (!nh_.getParam("task_main/max_distance", max_distance)) {
        ROS_ERROR("Failed to get param 'max_distance'");
        max_distance = 0.1; // 默认值
    }
    if (!nh_.getParam("task_main/min_angle", min_angle)) {
        ROS_ERROR("Failed to get param 'min_angle'");
        min_angle = 0.2; // 默认值
    }
    if (!nh_.getParam("task_main/size_threshold", size_threshold)) {
        ROS_ERROR("Failed to get param 'size_threshold'");
        size_threshold = 100; // 默认值
    }
    if (!nh_.getParam("task_main/step", step)) {
        ROS_ERROR("Failed to get param 'step'");
        step = 10; // 默认值
    }
    pcl_match->SetParams(min_distance, max_distance, min_angle, size_threshold, step );
}


/**
 * @brief 调试指令回调入口
 * @param msg-指令字符串
 * @return
 * */
void TaskMain::GetTestCmdCallback(const std_msgs::String::ConstPtr& msg)
{
    int ret = 0;
    float spin = 0.0;
    ACTION_DATA_S action;
    ROS_WARN("get test cmd[%s]", msg->data.c_str());
    if (strstr(msg->data.c_str(), "docking") != NULL) {
        ros::Rate r(20);
        ros::NodeHandle n;
        // 计算直角交点(x0,y0)=1.862341, 1.191646，目标点 1.36， 0.71
        params->get_cross_point_flag_ = true;
        // 设置到导航点到（目标点附近）
        nav_controller->PublishGoalCmd(1.36, 0.31, 3.14);
        // 自定义对接运动
        while (n.ok()) {
            ret = nav_controller->DockingTaskAction_WithPCL(&action,g_pose_2d, params->g_cross_point_);
            if (!ret)
                nav_controller->PublishMotorCmd(action.v, action.w);//不检测尾部距离
            else {
                nav_controller->PublishMotorCmd(0, 0);
                break;
            }
            ros::spinOnce();
            r.sleep();
        }
        params->get_cross_point_flag_ = false;
    }
}

void TaskMain::run()
{

}

/**
 * @brief 获取carto位置信息
 * @param pose-当前坐标
 * @return
 * */
int pos_count = 0;
void TaskMain::GetCartographPoseCmdCallback(const geometry_msgs::PoseStamped::ConstPtr& pose)
{
    g_carto_pose_.pose = pose->pose;
    g_pose_2d.x = g_carto_pose_.pose.position.x;
    g_pose_2d.y = g_carto_pose_.pose.position.y;
    g_pose_2d.yaw = tools->QuaternionTransToYaw(pose->pose.orientation);

    pos_count++;
    if (pos_count % 8 == 0) //N次发一次位置
    {
        pos_count = 0;
        PublishPoseCmd(g_carto_pose_.pose.position.x, g_carto_pose_.pose.position.y, g_pose_2d.yaw * RADIAN_TO_DEGREE);
    }
}

/**
 * @brief 发布位置信息
 * @param (x, y) - 坐标
 * 		  a - 角度
 * @return
 * */
void TaskMain::PublishPoseCmd(float x, float y, float a)
{
    task_main::DMPose pose;
    pose.x = x;
    pose.y = y;
    pose.angle = a;
    pub_pose_.publish(pose);
}

